﻿/*
 * altivario.c
 *
 * Created: 2013-08-23 오전 11:44:38
 *  Author: akkdong
 */ 

#include <math.h>
#include "altivario.h"


void altivario_init(AltiVario * altivario, double seaLevelPressure, double positionNoise, double accNoise, double dampFactor)
{
	altivario->seaLevelPressure = seaLevelPressure;

	altivario->positionNoise = positionNoise; // 0.2;
	altivario->accelerationNoise = accNoise; // 1.0;
	altivario->altDamp = dampFactor; // 0.05;
	altivario->varDamp = dampFactor; // 0.05;
	altivario->dampedStarted = 0;

	kalman_init(&altivario->kalman, altivario->accelerationNoise);
}

double altivario_addPressure(AltiVario * altivario, double pressure, double dt)
{
	altivario->rawPressure = pressure;
	altivario->rawAltitude =  44330.0 * (1 - pow((pressure / altivario->seaLevelPressure), 0.190295));

//	kalman_update(&altivario->kalman, altivario->rawAltitude, altivario->positionNoise, dt /* or 0.02, 0.03, 0.04 */);
	kalman_update(&altivario->kalman, altivario->rawAltitude, altivario->positionNoise, 0.02);

	altivario->kalmanAltitude = kalman_getXAbs(&altivario->kalman);
	altivario->kalmanVario = kalman_getXVel(&altivario->kalman);

	if( altivario->dampedStarted )
	{
		// low-pass filter
		altivario->dampedAltitude = altivario->dampedAltitude + altivario->altDamp * (altivario->rawAltitude - altivario->dampedAltitude);
		altivario->dampedVario = altivario->dampedVario + altivario->varDamp * (altivario->kalmanVario - altivario->dampedVario);
	}
	else
	{
		altivario->dampedAltitude = altivario->kalmanAltitude; // altivario->rawAltitude;
		altivario->dampedVario = altivario->kalmanVario;
		altivario->dampedStarted = 1;
	}
	
	return altivario->rawAltitude;
}

double altivario_setAltitude(AltiVario * altivario, double altitude)
{
	if( altivario->dampedStarted )
	{
		altivario->rawAltitude = altitude;
		altivario->seaLevelPressure = altivario->rawPressure / pow(1.0 - (altitude / 44330.0), 5.255);
	}
	
	return altivario->seaLevelPressure;
}

void altivario_setSealLevelPressure(AltiVario * altivario, double seaLevelPressure)
{
	altivario->seaLevelPressure = seaLevelPressure;
	altivario->dampedStarted = 0;
}

double altivario_getRawPressure(AltiVario * altivario)
{
	return altivario->rawPressure;
}

double altivario_getRawAltitude(AltiVario * altivario)
{
	return altivario->rawAltitude;
}

double altivario_getAltitude(AltiVario * altivario)
{
	return altivario->kalmanAltitude;
}

double altivario_getDampedAltitude(AltiVario * altivario)
{
	return altivario->dampedAltitude;
}

double altivario_getVario(AltiVario * altivario)
{
	return altivario->kalmanVario;
}

double altivario_getDampedVario(AltiVario * altivario)
{
	return altivario->dampedVario;
}
